
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>

#include <VirtualRobot/VirtualRobot.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <VirtualRobot/RobotNodeSet.h>



using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    cout << " --- MMM extractmmm --- " << endl;
    cout << "extract a motion from a mmm file and save it as an another mmm file" << endl;

    // we are expecting three arguments, and that is the filename of the file to convert
    if (argc<4)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: extractMotion %motionfile_in %outputfile %motionName" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename = std::string(argv[1]);
    std::string filenameOut = std::string(argv[2]);
    std::string motionname = std::string(argv[3]);

    cout << "Filenames provided: [" << filename << "] -> [" << filenameOut << "]" << endl;

    if(filenameOut.empty())
    {
        filenameOut = motionname + "output.xml";
    }

    if (!filename.empty())
    {
        // input file 1
        QFileInfo fileInfo(filename.c_str());
        std::string suffix(fileInfo.suffix().toLatin1());

        // transform suffixes to lowerCase
        std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);

        // For now, we just concatenate MMM motion files, so we check for correct suffixes
        if ((suffix.compare("xml")!=0))
        {
            cout << "Expected MMM Motion files as input (.xml). Aborting..." << endl;
            return -1;
        }
        // load input files
        MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());

        if(motionname.empty())
        {
            std::cout << "Please specify the motion name to be extracted (from the following list): " << std::endl;

            std::vector<std::string> motionNames = r->getMotionNames(filename);

            for(size_t i = 0; i < motionNames.size(); ++i)
            {
                std::cout << motionNames[i] << std::endl;
            }

            return -1;
        }

        MMM::LegacyMotionPtr motion = r->loadMotion(filename, motionname);
        motion->print(filenameOut);

    }

    return 0;
}
